
//***************************************************************************
//                             
//***************************************************************************

//---------------------------------------------------------------------------
//  Includes
//---------------------------------------------------------------------------

#include <stdlib.h>

#include "SphereFilledBody.h"

namespace vhand
{

static cudaError err;


//***************************************************************************
//                             
//***************************************************************************

float* SphereFilledBody::openPosArrayFromCGR( struct cudaGraphicsResource *geom_cgr,
                                              unsigned int possizeinbyte )
{
    // map the resource
    cudaGraphicsMapResources( 1,
                              &geom_cgr,
                              0 );
    err = cudaThreadSynchronize();
    if ( cudaSuccess != err)
    {
        fprintf(stderr, "Cuda error in file '%s' in line %i : %s.\n", __FILE__, __LINE__, cudaGetErrorString(err) );
    }

    // get the cuda-mapped resource's handle
    void *dataarray = NULL;
    size_t availablesizeinbyte = 0;
    cudaGraphicsResourceGetMappedPointer( &dataarray,
                                          &availablesizeinbyte,
                                          geom_cgr );
    err = cudaThreadSynchronize();
    if ( cudaSuccess != err)
    {
        fprintf(stderr, "Cuda error in file '%s' in line %i : %s.\n", __FILE__, __LINE__, cudaGetErrorString(err) );
    }

    float *posarray = static_cast<float*>( dataarray );
    if ( posarray == NULL )
    {
        // should handle with exception, do it later...
        // change the Exception.h's position, which should not belong to
        // GeoRender
        printf( "Errors while calling %s : %s :\n\tcudaGraphicsResourceGetMappedPointer fails.\n",
                __FILE__, __FUNCTION__ );
        exit( 0 );
    }

    if ( availablesizeinbyte != possizeinbyte )
    {
        printf( "The length of data fetched from cudaGraphicsResource is not the same as the original length of the array.\n" );
        printf( "Length of fetched buffer from cudaGraphicsResource: %d\n", 
                availablesizeinbyte );
        printf( "Length of original buffer: %d\n", possizeinbyte );
        exit( 0 );
    }

    return posarray;
}


void SphereFilledBody::closePosArrayFromCGR( struct cudaGraphicsResource *geom_cgr )
{
    //////////////////////////////////////////////////////////////////////
    // unmap the resource,
    // errors may occurs if the source is used by other programm while it's
    // not unmapped.
    cudaGraphicsUnmapResources( 1,
                                &geom_cgr,
                                0 );
    err = cudaThreadSynchronize();
    if ( cudaSuccess != err)
    {
        fprintf(stderr, "Cuda error in file '%s' in line %i : %s.\n", __FILE__, __LINE__, cudaGetErrorString(err) );
    }
}


SphereFilledBody::~SphereFilledBody()
{
    // release device memory
    
    // release host memory
}

void SphereFilledBody::setSpringArg( const AdjacencyType &type, float ks, float kd, float kv/* = 0 */ )
{
    if ( type == SPHERE_SPHERE )
    {
        _ks_sphr_sphr = ks;
        _kd_sphr_sphr = kd;
        _kv_sphr_sphr = kv;
    }
    else if ( type == SPHERE_BONES )
    {
        _ks_sphr_bone = ks;
        _kd_sphr_bone = kd;
    }
    else if ( type == SKIN_SKIN )
    {
        _ks_skin_skin = ks;
        _kd_skin_skin = kd;
    }
    else if ( type == SKIN_SPHERE )
    {
        _ks_skin_sphr = ks;
        _kd_skin_sphr = kd;
    }
    else
    {
        throw XError( "Errors while calling %s : %s :\n\tOnly 4 type Adjacency supported.\n",
                __FILE__, __FUNCTION__ );
    }
}

void SphereFilledBody::setHandGeometry( Geometry *bones_geom, Geometry *skin_geom )
{
    _bones_geom = bones_geom;
    _skin_geom = skin_geom;
}


/** 
 *
 *  The sequence of processing spheres/skin/bones's mesh makes a role
 *
 *  1) spheres
 *  2) skin
 *  3) bones
 * 
 */
void SphereFilledBody::initHost( void )
{
    ///////////////////////////////////////////////////////////////////////////////////
    // initial massspringsystem in host memory
    //
    _sss.setDeltaT( _dt );
    _sss.setStepNum( _stepnum );
    _sss.setMassRadiusOverlapArg( _kv_sphr_sphr );

    // 1) append the mass wrb. mesh into the mss
    // The sequence of processing spheres/skin/bones's mesh makes a role
    //
    // a) spheres
    // b) skin
    // c) bones
    //

#if 0
    /////////////////////////////
    // debuging
    _spheres_index = 0;
    // The greatest number of spheres, with which the programm can work
    // correctly.  With 20196 and more sphere the programm fails(
    // mss.transformMassPos fails, why? )
    _spheres_size = 20195;
    for ( unsigned int i=0; i < _spheres_size; i++ )
    {
        _sss.addOneMass( Point3( 0.0, 0.0, 0.0 ),
                         0.0 );
    }
    // debuging...
    /////////////////////////////
#endif

//#if 0
    _spheres_index = 0;
    _spheres_size = _allsphere.size();
    for ( unsigned int i=0; i < _spheres_size; i++ )
    {
        _sss.addNewSphere( Point3( _allsphere[i].pX, 
                                   _allsphere[i].pY,
                                   _allsphere[i].pZ ),
                           _allsphere[i].mR );
    }
//#endif
    _skin_index = _spheres_size;
    _skin_size = _skin_geom->getPositions()->getArray()->size();
    for ( unsigned int i=0; i < _skin_size; i++ )
    {
        _sss.addNewSphere( (*_skin_geom->getPositions()->getArray())[i],
                           0.0 );
    }
    _bones_index = _spheres_size + _skin_size;
    _bones_size = _bones_geom->getPositions()->getArray()->size();
    for ( unsigned int i=0; i < _bones_size; i++ )
    {
        _sss.addNewSphere( (*_bones_geom->getPositions()->getArray())[i],
                           0.0 );
    }

    // 2) append the springs wrt. topology into the mss
    for ( unsigned int i=0; i < _topo.getAllAdjacency()->size(); i++ )
    {
        const Adjacency &adjc = (*_topo.getAllAdjacency())[i];
        if ( adjc.getType() == SPHERE_SPHERE )
        {
            //continue;
            _sss.addNewSpring( _ks_sphr_sphr,
                               _kd_sphr_sphr,
                               _spheres_index + adjc.getOut(),
                               _spheres_index + adjc.getIn() );
        }
        else if ( adjc.getType() == SPHERE_BONES )
        {
            //continue;
            _sss.addNewSpring( _ks_sphr_bone,
                               _kd_sphr_bone,
                               _spheres_index + adjc.getOut(),
                               _bones_index + adjc.getIn() );
        }
        else if ( adjc.getType() == SKIN_SKIN )
        {
            _sss.addNewSpring( _ks_skin_skin,
                               _kd_skin_skin,
                               _skin_index + adjc.getOut(),
                               _skin_index + adjc.getIn() );
        }
        else if ( adjc.getType() == SKIN_SPHERE )
        {
            //continue;
            _sss.addNewSpring( _ks_skin_sphr,
                               _kd_skin_sphr,
                               _skin_index + adjc.getOut(),
                               _spheres_index + adjc.getIn() );
        }
        else if ( adjc.getType() == MAP_SPHERE_BONES || 
                  adjc.getType() == MAP_SKIN_SPHERE )
        {
            // do nothing.
        }
        else
        {
            throw XError( "Errors while calling %s : %s :\n\tOnly 4 type Adjacency supported.\n",
                          __FILE__, __FUNCTION__ );
        }
    }

    // debug...
    printf( "spheres_index: %d\n", _spheres_index );
    printf( "spheres_size : %d\n", _spheres_size );
    printf( "skin_index   : %d\n", _skin_index );
    printf( "skin_size    : %d\n", _skin_size );
    printf( "bones_index  : %d\n", _bones_index );
    printf( "bones_size   : %d\n", _bones_size );
    printf( "stepnum      : %i\n", _stepnum );
    printf( "incremental  : %f\n", _dt );
    printf( "ks_sphr_sphr : %f\n", _ks_sphr_sphr );
    printf( "kd_sphr_sphr : %f\n", _kd_sphr_sphr );
    printf( "kv_sphr_sphr : %f\n", _kv_sphr_sphr );

    return;
}


void SphereFilledBody::initDevice( void )
{
    ///////////////////////////////////////////////////////////////////////// 
    // register bones' geometry to cuda
    //
    _bones_pos_vbo_id = _bones_geom->editPositions()->getVBOId();
    _bones_pos_vbo_sizeinbyte = _bones_geom->editPositions()->getVBOSizeInByte();
    cudaGraphicsGLRegisterBuffer( &_bones_pos_cgr,
                                  _bones_pos_vbo_id,
                                  cudaGraphicsMapFlagsNone );
    err = cudaThreadSynchronize();
    if ( cudaSuccess != err)
    {
        throw XError( "Cuda error in file '%s' in line %i : %s.\n", __FILE__, __LINE__, cudaGetErrorString(err) );
    }

    // register skin's geometry to cuda
    //
    _skin_pos_vbo_id = _skin_geom->editPositions()->getVBOId();
    _skin_pos_vbo_sizeinbyte = _skin_geom->editPositions()->getVBOSizeInByte();
    cudaGraphicsGLRegisterBuffer( &_skin_pos_cgr,
                                  _skin_pos_vbo_id,
                                  cudaGraphicsMapFlagsNone );
    err = cudaThreadSynchronize();
    if ( cudaSuccess != err)
    {
        fprintf(stderr, "Cuda error in file '%s' in line %i : %s.\n", __FILE__, __LINE__, cudaGetErrorString(err) );
    }

    // sss uploads his data from host to device
    _sss.initDevice();
    _sss.setFixedSphereRange( _bones_index, _bones_size );

    return;
}


void SphereFilledBody::deformation( const IndexRanges *rs_bones,
                                    const IndexRanges *rs_skin,
                                    const IndexRanges *rs_spheres,
                                    const Matrix4 &updatemat )
{
    printf( "--- SphereFilledBody::deformation() ---\n" );

#if 0
    ////////////////////////////////////////////
    // debug...

    printf( "debuging info:\n" );
    std::vector<Point3> tmp1;
    _sss.getPositionHost( IndexRange( 33665, 10 ), &tmp1 );
    for ( unsigned int i=0; i < tmp1.size(); i ++ )
    {
        printf( "%d -th Point: ", i );
        print( tmp1[i] );
    }
    
    //
    ////////////////////////////////////////////
#endif

    float h_matarray[ 16 ];
    defE::loadMatrixArray( h_matarray, updatemat );
    // debug...
    for ( unsigned int i=0; i<4; i++ )
    {
        printf( "[ " );
        for ( unsigned int j=0; j<4; j++ )
        {
            printf( "%f, ", h_matarray[ i+ j*4  ] );

            if ( j == 3 )
                printf( " ]\n" );
        }
    }

    float *matarray;
    cudaMalloc( &matarray, 16*sizeof(float) );
    cudaMemcpy( matarray, h_matarray, 16*sizeof(float), cudaMemcpyHostToDevice );
    cudaError err = cudaThreadSynchronize();
    if ( cudaSuccess != err)
    {
        fprintf(stderr, "Cuda error in file '%s' in line %i : %s.\n", __FILE__, __LINE__, cudaGetErrorString(err) );
    }


//#if 0
    // for bones
    printf( "deform bones:\n" );
    for ( std::vector<IndexRange>::const_iterator iter = rs_bones->getArray()->begin();
          iter != rs_bones->getArray()->end(); iter ++ )
    {
        printf( "[ %d, %d ] ", iter->first, iter->first+iter->second-1 );
        _sss.deformation( IndexRange( _bones_index + iter->first, iter->second ),
                           matarray );
    }
    // for skin
    printf( "\ndeform skin:\n" );
    for ( std::vector<IndexRange>::const_iterator iter = rs_skin->getArray()->begin();
          iter != rs_skin->getArray()->end(); iter ++ )
    {
        printf( "[ %d, %d ] ", iter->first, iter->first+iter->second-1 );
        _sss.deformation( IndexRange( _skin_index + iter->first, iter->second ),
                          matarray );
    }
    // for spheres
    printf( "\ndeform spheres:\n" );
    for ( std::vector<IndexRange>::const_iterator iter = rs_spheres->getArray()->begin();
          iter != rs_spheres->getArray()->end(); iter ++ )
    {
        printf( "[ %d, %d ] ", iter->first, iter->first+iter->second-1 );
        _sss.deformation( IndexRange( _spheres_index + iter->first, iter->second ),
                          matarray );
    }
    printf( "\n" );

    cudaFree( matarray );
    //delete [] h_matarray;
    return;
}


void SphereFilledBody::simulation()
{
    printf( "--- SphereFilledBody::simulation() ---\n" );

    ///////////////////////////////////////////////////////////////////////// 
    //
    // I. processing regular transformation as if the hand model is rigid object
    //
    // 1) take the state of mss at the current frame as the stabil state
    //_sss.resetInitialState();
    _sss.stopAllSphere();

    ///////////////////////////////////////////////////////////////////////// 
    //
    // III. Then handle the mass-spring system simulation for more real modeling
    // for joints
    //
    _sss.simulation();

    return;
}


void SphereFilledBody::updateGeometry()
{
    printf( "--- SphereFilledBody::updateGeometry() ---\n" );

#if 0
    ////////////////////////////////////////////
    // debug...

    printf( "debuging info:\n" );
    std::vector<Point3> tmp1;
    //_sss.getPositionHost( IndexRange( 33665, 10 ), &tmp1 );
    _sss.getPositionHost( IndexRange( 0, 10 ), &tmp1 );
    for ( unsigned int i=0; i < tmp1.size(); i ++ )
    {
        printf( "%d -th Point: ", i );
    }
    
    //
    ////////////////////////////////////////////
#endif

#if 0
    ////////////
    // debug...
    //
    _bones_pos_vbo_id = _bones_geom->editPositions()->getVBOId();
    _bones_pos_vbo_sizeinbyte = _bones_geom->editPositions()->getVBOSizeInByte();
    cudaGraphicsGLRegisterBuffer( &_bones_pos_cgr,
                                  _bones_pos_vbo_id,
                                  cudaGraphicsMapFlagsNone );
    err = cudaThreadSynchronize();
    if ( cudaSuccess != err)
    {
        throw XError( "Cuda error in file '%s' in line %i : %s.\n", __FILE__, __LINE__, cudaGetErrorString(err) );
    }

    // register skin's geometry to cuda
    //
    _skin_pos_vbo_id = _skin_geom->editPositions()->getVBOId();
    _skin_pos_vbo_sizeinbyte = _skin_geom->editPositions()->getVBOSizeInByte();
    cudaGraphicsGLRegisterBuffer( &_skin_pos_cgr,
                                  _skin_pos_vbo_id,
                                  cudaGraphicsMapFlagsNone );
    err = cudaThreadSynchronize();
    if ( cudaSuccess != err)
    {
        fprintf(stderr, "Cuda error in file '%s' in line %i : %s.\n", __FILE__, __LINE__, cudaGetErrorString(err) );
    }
    //
    // debug...
    ////////////
#endif
    
    ///////////////////////////////////////////////////////////////////////// 
    //
    // V. Change the rendered-out geometry object(bones/skin/sss)
    //
    float *bonespos = openPosArrayFromCGR( _bones_pos_cgr, _bones_pos_vbo_sizeinbyte );
    float *skinpos = openPosArrayFromCGR( _skin_pos_cgr, _skin_pos_vbo_sizeinbyte );

    // 1) then change bones and skin
    _sss.getPositionDevice( IndexRange( _bones_index, _bones_size ),
                            bonespos );
    _sss.getPositionDevice( IndexRange( _skin_index, _skin_size ),
                            skinpos );

    closePosArrayFromCGR( _bones_pos_cgr );
    closePosArrayFromCGR( _skin_pos_cgr );

    // debug...
    _sss.updateGeometry();

    ///////////////////////////////////////////////////////////////////////// 
    //
    // VI. Release resources.
    //
    err = cudaThreadSynchronize();
    if ( cudaSuccess != err)
    {
        fprintf(stderr, "Cuda error in file '%s' in line %i : %s.\n", __FILE__, __LINE__, cudaGetErrorString(err) );
    }

    return;
}

} // namespace vhand
